from openravepy import *
from numpy import *

f = file("/home/grasping/Downloads/primesense_data/apple.pcd")

lines = f.readlines()
print "NumLines: ",len(lines)

coords = [(float(x),float(y),float(z)) for x,y,z in (line.split()[:3] for line in lines[3:])]
colors = [(float(r),float(g),float(b)) for r,g,b in (line.split()[5:8] for line in lines[3:])]
env = Environment()

env.SetViewer('qtcoin')
ct = array([[-0.03955483, -0.97601718, -0.21407017, -0.02339525],
 [-0.99849898,  0.03048521,  0.04550455,  0.10448422],
 [-0.03788722,  0.21554874, -0.97575802, -0.07398532],
 [ 0.,          0.,          0.,          1.        ]])
env.SetCamera(ct)
h1 = env.plot3(points=array(coords),pointsize=1)
raw_input("Press Enter to continue...")
h1 = None
